/*
 * VisionManager.cpp
 *
 *  Created on: Apr 19, 2010
 *      Author: Soheil Damangir
 */
#include "image.h"
#include "log.h"
#include "Detector.h"
#include "StripeDetector.h"
#include "Image2DDD.h"
#include "VisionManager.h"
#include "../Util/Conf.h"

using kthrobot::Log;
namespace Group5 {

VisionManager::VisionManager()
{
  Log::Write("----------------Initializing VisionManager-----------------");
  {
    ifstream ifs(Conf::get().read<std::string>("BallTrainFile").c_str());
    if (!ifs.is_open())
    {
      Log::Write("Unable to load vision training file.");
      kthrobot::Init::SignalShutdown();
      return;
    }
    ball_detector = new Detector(ifs);
    Log::Write("Loading Ball Detector");
    ifs.close();
  }
  {
    ifstream ifs(Conf::get().read<std::string>("BombTrainFile").c_str());
    if (!ifs.is_open())
    {
      kthrobot::Log::Write("Unable to load vision training file.");
      kthrobot::Init::SignalShutdown();
      return;
    }
    bomb_detector = new Detector(ifs);
    Log::Write("Loading Bomb Detector");
    ifs.close();
  }
  {
    ifstream ifs(Conf::get().read<std::string>("StripeTrainFile").c_str());
    if (!ifs.is_open())
    {
      kthrobot::Log::Write("Unable to load vision training file.");
      kthrobot::Init::SignalShutdown();
      return;
    }
    stripe_detector = new StripeDetector(ifs);
    Log::Write("Loading Stripe Detector");
    ifs.close();
  }

  Image2DDD::t11 =  Conf::get().read<int>("T_11");
  Image2DDD::t12 =  Conf::get().read<int>("T_12");
  Image2DDD::t21 =  Conf::get().read<int>("T_21");
  Image2DDD::t22 =  Conf::get().read<int>("T_22");

  Image2DDD::bx =  Conf::get().read<int>("B_X");
  Image2DDD::by =  Conf::get().read<int>("B_Y");

  jpeg_buffers = new JPEGBuffers;
  jpeg_queue = new JPEGQueue;

  cam_reader = new CameraReader(Conf::get().read<std::string>("CameraIP"), jpeg_buffers, jpeg_queue);
  processor = new ImageProcessor(jpeg_queue, this);
  Log::Write("-----------------------------------------------------------");
}

void VisionManager::Up()
{
  cam_reader->Start();
  processor->Start();
  Log::Write("VisionManager is running");
}
void VisionManager::Down()
{
  cam_reader->SignalStop();
  processor->SignalStop();
  Log::Write("VisionManager down");
}

void VisionManager::ProcessImage(kthrobot::Image*image)
{
  last_image = image;
  normalize_image(last_image);
  ball_detector->detect(*last_image, ball_result,"Ball_detector");
  {
    int x=0,y=0;
    Image2DDD::Map(ball_result.x,ball_result.y, x, y);
    ball_result.x = x;
    ball_result.y = y;
  }

  if (ball_result.detected)
    stripe_detector->detect(*last_image, stripe_result, "Stripe_detector");
  bomb_detector->detect(*last_image, bomb_result, "Bomb_detector");
}

DetectionResults const & VisionManager::getBallResult()
{
  return ball_result;
}
DetectionResults const & VisionManager::getBombResult()
{
  return bomb_result;
}
DetectionResults const & VisionManager::getStripeResult()
{
  return stripe_result;
}

VisionManager::~VisionManager()
{
  delete processor;
  delete cam_reader;
  delete jpeg_buffers;
  delete jpeg_queue;
  delete ball_detector;
  delete bomb_detector;
  delete stripe_detector;
  Log::Write("Deinitializing VisionManager");
}
}
